/*
 * @Author: laladuduqq
 * @Date: 2024-12-23 23:16:42
 * @LastEditors: laladuduqq
 * @LastEditTime: 2025-01-19 20:40:54
 */
#include "board_com.h"
#include "imu.h"
#include "message.h"
#include "offline.h"
#include "robotdef.h"
#include "rtthread.h"
#include "rttypes.h"
#include "sbus.h"
#include "user_lib.h"
#include "vcom.h"
#include "referee.h"


#define LOG_TAG              "robotcontrol"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>

//部分仅限内部使用函数声明
static float CalcOffsetAngle();
static void RemoteControlSet();

#ifdef ONE_BOARD
    static Publisher_t *gimbal_cmd_pub;            // 云台控制消息发布者
    static Gimbal_Ctrl_Cmd_s gimbal_cmd_send;      // 传递给云台的控制信息

    static Subscriber_t *gimbal_feed_sub;          // 云台反馈信息订阅者
    static Gimbal_Upload_Data_s gimbal_fetch_data; // 从云台获取的反馈信息

    static Publisher_t *shoot_cmd_pub;           // 发射控制消息发布者
    static Shoot_Ctrl_Cmd_s shoot_cmd_send;      // 传递给发射的控制信息

    static Publisher_t *chassis_cmd_pub;              // 底盘控制消息发布者
    static Chassis_Ctrl_Cmd_s chassis_cmd_send;      // 发送给底盘应用的信息,包括控制信息和UI绘制相关

    static Subscriber_t *chassis_feed_sub;          // 底盘反馈信息订阅者
    static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等

    void robot_control_init()
    {
        gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
        gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));

        shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));

        chassis_cmd_pub= PubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
        chassis_feed_sub = SubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
    }

    void robot_control_task(void *paramter)
    {
        SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
        SubGetMessage(chassis_feed_sub, &chassis_fetch_data);
        CalcOffsetAngle();
        RemoteControlSet();


        PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
        PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
        PubPushMessage(chassis_cmd_pub, (void *)&chassis_cmd_send);
    }
#endif

#ifndef ONE_BOARD

    #if defined (HERO_MODE) || defined (ENGINEER_MODE) || defined (INFANTRY_MODE)
        #if defined (GIMBAL_BOARD)
            static Publisher_t *gimbal_cmd_pub;            // 云台控制消息发布者
            static Gimbal_Ctrl_Cmd_s gimbal_cmd_send;      // 传递给云台的控制信息

            static Subscriber_t *gimbal_feed_sub;          // 云台反馈信息订阅者
            static Gimbal_Upload_Data_s gimbal_fetch_data; // 从云台获取的反馈信息

            static Publisher_t *shoot_cmd_pub;           // 发射控制消息发布者
            static Shoot_Ctrl_Cmd_s shoot_cmd_send;      // 传递给发射的控制信息

            static Chassis_Ctrl_Cmd_s Chassis_Ctrl_Cmd;      // 发送给底盘应用的信息,包括控制信息和UI绘制相关
            static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等

            void robot_control_init()
            {
                //订阅 发布注册
                gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
                gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
                shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));

                //板间通讯初始化
                board_com.board_tx.can_handle=&can2_bus;
                board_com.board_tx.txmsg.id=GIMBAL_ID;
                board_com.send_data_len = sizeof(Chassis_Ctrl_Cmd_s);
                board_com.recv_data_len = sizeof(Chassis_Upload_Data_s);
                board_com.recv_buf_len = board_com.recv_data_len + CAN_COMM_OFFSET_BYTES; // head + datalen + crc8 + tail
                board_com.send_buf_len = board_com.send_data_len + CAN_COMM_OFFSET_BYTES; 
                board_com.raw_sendbuf[1] = board_com.send_data_len; // datalen
                board_com.raw_sendbuf[board_com.send_data_len + CAN_COMM_OFFSET_BYTES - 1] = CAN_COMM_TAIL;
            }
        #else
            void robot_control_init()
            {
                //板间通讯初始化
                board_com.board_tx.can_handle=&can2_bus;
                board_com.board_tx.txmsg.id=CHASSIS_ID;
                board_com.send_data_len = sizeof(Chassis_Upload_Data_s);
                board_com.recv_data_len = sizeof(Chassis_Ctrl_Cmd_s);
                board_com.recv_buf_len = board_com.recv_data_len + CAN_COMM_OFFSET_BYTES; // head + datalen + crc8 + tail
                board_com.send_buf_len = board_com.send_data_len + CAN_COMM_OFFSET_BYTES; 
                board_com.raw_sendbuf[1] = board_com.send_data_len; // datalen
                board_com.raw_sendbuf[board_com.send_data_len + CAN_COMM_OFFSET_BYTES - 1] = CAN_COMM_TAIL;
            }
        #endif

    #endif 

    #if defined (DRONE_MODE)
        static Publisher_t *gimbal_cmd_pub;            // 云台控制消息发布者
        static Gimbal_Ctrl_Cmd_s gimbal_cmd_send;      // 传递给云台的控制信息

        static Subscriber_t *gimbal_feed_sub;          // 云台反馈信息订阅者
        static Gimbal_Upload_Data_s gimbal_fetch_data; // 从云台获取的反馈信息

        static Publisher_t *shoot_cmd_pub;           // 发射控制消息发布者
        static Shoot_Ctrl_Cmd_s shoot_cmd_send;      // 传递给发射的控制信息

        static Subscriber_t *chassis_feed_sub;          // 底盘反馈信息订阅者
        static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
        void robot_control_init()
        {
            gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
            gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));

            shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));

            chassis_feed_sub = SubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
        }

        void robot_control_task(void *paramter)
        {
            CalcOffsetAngle();
            RemoteControlSet();
            SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
            SubGetMessage(chassis_feed_sub, &chassis_fetch_data);


            PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
            PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
            PubPushMessage(chassis_cmd_pub, (void *)&chassis_cmd_send);
        }
    #endif

    #if defined (SENTRY_MODE)
        #if defined (SENTRY_BOARD_SMALL_YAW_1)
            static Publisher_t *gimbal_cmd_pub;            // 云台控制消息发布者

            static Publisher_t *shoot_cmd_pub;           // 发射控制消息发布者
            
            static Small_Yaw_Heart_Pack_s Small_Yaw_Heart_Pack; //small yaw online heart pack

            static Gimbal_Ctrl_Cmd_s Small_Yaw_Ctrl_Cmd;
            static Shoot_Ctrl_Cmd_s Shoot_Ctrl_Cmd;
            void robot_control_init()
            {
                gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));

                shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));

                board_com.board_tx.can_handle=&can2_bus;
            }

            void robot_control()
            {
                static rt_uint8_t buffer[7]={0};
                board_send(BOARD_SMALL_YAW_1_ID,buffer,GIMBAL_TAIL_CHECK_ID);

                Small_Yaw_Ctrl_Cmd = *(Gimbal_Ctrl_Cmd_s *)CANCommGet(GIMBAL_TAIL_CHECK_ID);
                
                if (Small_Yaw_Ctrl_Cmd.gimbal_mode ==GIMBAL_KEEPING_SMALL_YAW_2 || Small_Yaw_Ctrl_Cmd.gimbal_mode ==GIMBAL_AUTO_MODE)
                {
                    //Shoot_Ctrl_Cmd = *(Shoot_Ctrl_Cmd_s *)CANCommGet(SHOOT_TAIL_CHECK_ID);
                    Shoot_Ctrl_Cmd.shoot_mode =SHOOT_OFF;
                    Shoot_Ctrl_Cmd.friction_mode = FRICTION_OFF;
                    Shoot_Ctrl_Cmd.load_mode = LOAD_STOP;
                }
                else 
                {
                    Shoot_Ctrl_Cmd.shoot_mode =SHOOT_OFF;
                    Shoot_Ctrl_Cmd.friction_mode = FRICTION_OFF;
                    Shoot_Ctrl_Cmd.load_mode = LOAD_STOP;
                }

                // if (Small_Yaw_Ctrl_Cmd.gimbal_mode ==GIMBAL_AUTO_MODE) 
                // {
                //     offline_event_enable(OFFLINE_MINIPC_HEART);
                //     if (Vision_Recv.fire_advice) {Shoot_Ctrl_Cmd.load_mode =LOAD_BURSTFIRE;}else{Shoot_Ctrl_Cmd.load_mode =LOAD_STOP;}
                // }
                // else {offline_event_disable(OFFLINE_MINIPC_HEART);}

                PubPushMessage(gimbal_cmd_pub, (void *)&Small_Yaw_Ctrl_Cmd);
                PubPushMessage(shoot_cmd_pub, (void *)&Shoot_Ctrl_Cmd);
            }

        #elif defined (SENTRY_BOARD_SMALL_YAW_2)
            static Publisher_t *gimbal_cmd_pub;            // 云台控制消息发布者

            static Publisher_t *shoot_cmd_pub;           // 发射控制消息发布者
            
            static Small_Yaw_Heart_Pack_s Small_Yaw_Heart_Pack; //small yaw online heart pack

            static Gimbal_Ctrl_Cmd_s Small_Yaw_Ctrl_Cmd;
            static Shoot_Ctrl_Cmd_s Shoot_Ctrl_Cmd;
            void robot_control_init()
            {
                gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));

                shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));

                board_com.board_tx.can_handle=&can2_bus;
            }

            void robot_control()
            {
                static rt_uint8_t buffer[7]={0};
                board_send(BOARD_SMALL_YAW_2_ID,buffer,GIMBAL_TAIL_CHECK_ID);

                Small_Yaw_Ctrl_Cmd = *(Gimbal_Ctrl_Cmd_s *)CANCommGet(GIMBAL_TAIL_CHECK_ID);
                if (Small_Yaw_Ctrl_Cmd.gimbal_mode ==GIMBAL_KEEPING_SMALL_YAW_1 || Small_Yaw_Ctrl_Cmd.gimbal_mode ==GIMBAL_AUTO_MODE)
                {
                    Shoot_Ctrl_Cmd = *(Shoot_Ctrl_Cmd_s *)CANCommGet(SHOOT_TAIL_CHECK_ID);
                }
                else 
                {
                    Shoot_Ctrl_Cmd.shoot_mode =SHOOT_OFF;
                    Shoot_Ctrl_Cmd.friction_mode = FRICTION_OFF;
                    Shoot_Ctrl_Cmd.load_mode = LOAD_STOP;
                }

                if (Small_Yaw_Ctrl_Cmd.gimbal_mode ==GIMBAL_AUTO_MODE) {
                    offline_event_enable(OFFLINE_MINIPC_HEART);
                    if (Vision_Recv.fire_advice) {Shoot_Ctrl_Cmd.load_mode =LOAD_BURSTFIRE;}else{Shoot_Ctrl_Cmd.load_mode =LOAD_STOP;}
                }
                else {offline_event_disable(OFFLINE_MINIPC_HEART);}

                PubPushMessage(gimbal_cmd_pub, (void *)&Small_Yaw_Ctrl_Cmd);
                PubPushMessage(shoot_cmd_pub, (void *)&Shoot_Ctrl_Cmd);
            }

        #elif defined (SENTRY_BOARD_BIG_YAW)
            static Publisher_t *gimbal_cmd_pub;            // 云台控制消息发布者
            static Gimbal_Ctrl_Cmd_s gimbal_cmd_send;      // 传递给云台的控制信息

            static Subscriber_t *gimbal_feed_sub;          // 云台反馈信息订阅者
            static Gimbal_Upload_Data_s gimbal_fetch_data; // 从云台获取的反馈信息

            static Gimbal_Ctrl_Cmd_s Small_Yaw_Ctrl_Cmd;  //small yaw gimbal 消息
            static Shoot_Ctrl_Cmd_s Shoot_Ctrl_Cmd;          //small yaw shoot 消息
            static Chassis_Ctrl_Cmd_s Chassis_Ctrl_Cmd;      //发送给底盘的消息
            static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
            void robot_control_init()
            {
                gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
                gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));

                board_com.board_tx.can_handle=&can2_bus;
            }

            void robot_control()
            {
                SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
                Chassis_Ctrl_Cmd.offset_angle = CalcOffsetAngle();
                RemoteControlSet();
                PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);

                if (gimbal_cmd_send.gimbal_mode == GIMBAL_AUTO_MODE) 
                {
                    float tmpvx = Nav_Recv.vx * (-1);
                    float tmpvy = Nav_Recv.vy * (-1);
                    Chassis_Ctrl_Cmd.vx =tmpvx;
                    Chassis_Ctrl_Cmd.vy =tmpvy;
                    board_send(BOARD_BIG_YAW_ID,&Chassis_Ctrl_Cmd,CHASSIS_TAIL_CHECK_ID);
                    offline_event_enable(OFFLINE_MINIPC_HEART);
                }
                else 
                {
                    offline_event_disable(OFFLINE_MINIPC_HEART);
                    board_send(BOARD_BIG_YAW_ID,&Chassis_Ctrl_Cmd,CHASSIS_TAIL_CHECK_ID);
                }

                Shoot_Ctrl_Cmd.rest_heat_1 = board_com.Chassis_Upload_Data.shooter_17mm_1_barrel_heat;
                Shoot_Ctrl_Cmd.rest_heat_2 = board_com.Chassis_Upload_Data.shooter_17mm_2_barrel_heat;
                board_send(BOARD_BIG_YAW_ID,&Small_Yaw_Ctrl_Cmd,GIMBAL_TAIL_CHECK_ID);     
                board_send(BOARD_BIG_YAW_ID,&Shoot_Ctrl_Cmd,SHOOT_TAIL_CHECK_ID);
            }            
        #elif defined (SENTRY_BOARD_CHASSIS)
            static Chassis_Upload_Data_s Chassis_Upload_Data; 
            void robot_control_init()
            {
                //板间通讯初始化
                board_com.board_tx.can_handle=&can2_bus;
            }

            void robot_control()
            {
                chassis_to_gimbal(&Chassis_Upload_Data);
                board_send(BOARD_CHASSIS_ID,&Chassis_Upload_Data,CHASSIS_UPLOAD_DATA_CHECK_ID);
            }    

        #endif
    
    #endif

#endif 


/**
 * @brief 根据gimbal app传回的当前电机角度计算和零位的误差
 *        单圈绝对角度的范围是0~360,说明文档中有图示
 *
 */
#if defined (SENTRY_BOARD_CHASSIS) || defined (CHASSIS_BOARD) || defined (SENTRY_BOARD_SMALL_YAW_1) || defined (SENTRY_BOARD_SMALL_YAW_2)

#else 
static float CalcOffsetAngle()
{
    static float offsetangle;
    // 从云台获取的当前yaw电机单圈角度 gimbal_fetch_data.yaw_motor_single_round_angle 
#if YAW_ECD_GREATER_THAN_4096                               // 如果大于180度
    if (gimbal_fetch_data.yaw_motor_single_round_angle > YAW_ALIGN_ANGLE && gimbal_fetch_data.yaw_motor_single_round_angle <= 180.0f + YAW_ALIGN_ANGLE)
    {    
        offsetangle = gimbal_fetch_data.yaw_motor_single_round_angle - YAW_ALIGN_ANGLE;
        return offsetangle;
    }
    else if (gimbal_fetch_data.yaw_motor_single_round_angle > 180.0f + YAW_ALIGN_ANGLE)
    {    
        offsetangle = gimbal_fetch_data.yaw_motor_single_round_angle - YAW_ALIGN_ANGLE - 360.0f;
        return offsetangle;
    }
    else
    {
        offsetangle = gimbal_fetch_data.yaw_motor_single_round_angle - YAW_ALIGN_ANGLE;
        return offsetangle;
    }
#else // 小于180度
    if (gimbal_fetch_data.yaw_motor_single_round_angle > YAW_ALIGN_ANGLE)
    {    
        offsetangle = gimbal_fetch_data.yaw_motor_single_round_angle - YAW_ALIGN_ANGLE;
        return offsetangle;
    }
    else if (gimbal_fetch_data.yaw_motor_single_round_angle <= YAW_ALIGN_ANGLE && gimbal_fetch_data.yaw_motor_single_round_angle >= YAW_ALIGN_ANGLE - 180.0f)
    {
        offsetangle = gimbal_fetch_data.yaw_motor_single_round_angle - YAW_ALIGN_ANGLE;
        return offsetangle;
    }
    else
    {
        offsetangle = gimbal_fetch_data.yaw_motor_single_round_angle - YAW_ALIGN_ANGLE + 360.0f;
        return offsetangle;
    }
#endif
}


#define DEAD_ZONE(value, min, max, center) ((value) >= (min) && (value) <= (max) ? (center) : (value))
static void RemoteControlSet()
{
    if (offline_manage[OFFLINE_SBUS].online_state == STATE_ONLINE)
    {
        Chassis_Ctrl_Cmd.vx = -3.0f * (float)(sbus.SBUS_CH.CH2 - SBUS_CHX_BIAS) / ((SBUS_CHX_UP - SBUS_CHX_DOWN) / 2.0f);
        Chassis_Ctrl_Cmd.vy = 3.0f * (float)(sbus.SBUS_CH.CH1 - SBUS_CHX_BIAS) / ((SBUS_CHX_UP - SBUS_CHX_DOWN) / 2.0f); //根据 2 * PI * wheel_radius * rpm / 60.0f可得最大线速度

        switch (sbus.SBUS_CH.CH6)
        {
            case SBUS_CHX_DOWN:
                if (sbus.SBUS_CH.CH9 < SBUS_CHX_BIAS)
                {
                    gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
                    rt_int16_t tmp = DEAD_ZONE(sbus.SBUS_CH.CH4, 1020, 1030, 1024);
                    gimbal_cmd_send.yaw -= 0.001f * (float)(tmp - SBUS_CHX_BIAS);
                    Small_Yaw_Ctrl_Cmd.gimbal_mode = GIMBAL_KEEPING;
                }
                break;

            case SBUS_CHX_BIAS:
                if (sbus.SBUS_CH.CH9 < SBUS_CHX_BIAS)
                {
                    gimbal_cmd_send.gimbal_mode = GIMBAL_KEEPING;
                    Small_Yaw_Ctrl_Cmd.gimbal_mode = GIMBAL_KEEPING_SMALL_YAW_2;
                    rt_int16_t tmp = DEAD_ZONE(sbus.SBUS_CH.CH4, 1020, 1030, 1024);
                    Small_Yaw_Ctrl_Cmd.yaw -= 0.001f * (float)(tmp - SBUS_CHX_BIAS);
                    tmp = DEAD_ZONE(sbus.SBUS_CH.CH3, 1020, 1030, 1024);
                    Small_Yaw_Ctrl_Cmd.pitch += 0.001f * (float)(tmp - SBUS_CHX_BIAS);
                    VAL_LIMIT(Small_Yaw_Ctrl_Cmd.yaw, SMALL_YAW_1_MIN_ANGLE, SMALL_YAW_1_MAX_ANGLE);
                    VAL_LIMIT(Small_Yaw_Ctrl_Cmd.pitch, SMALL_YAW_1_PITCH_MIN_ANGLE, SMALL_YAW_1_PITCH_MAX_ANGLE);
                }
                break;

            case SBUS_CHX_UP:
                if (sbus.SBUS_CH.CH9 < SBUS_CHX_BIAS)
                {
                    gimbal_cmd_send.gimbal_mode = GIMBAL_KEEPING;
                    Small_Yaw_Ctrl_Cmd.gimbal_mode = GIMBAL_KEEPING_SMALL_YAW_1;
                    rt_int16_t tmp = DEAD_ZONE(sbus.SBUS_CH.CH4, 1020, 1030, 1024);
                    Small_Yaw_Ctrl_Cmd.yaw -= 0.001f * (float)(tmp - SBUS_CHX_BIAS);
                    tmp = DEAD_ZONE(sbus.SBUS_CH.CH3, 1020, 1030, 1024);
                    Small_Yaw_Ctrl_Cmd.pitch += 0.001f * (float)(tmp - SBUS_CHX_BIAS);
                    VAL_LIMIT(Small_Yaw_Ctrl_Cmd.yaw, SMALL_YAW_2_MIN_ANGLE, SMALL_YAW_2_MAX_ANGLE);
                    VAL_LIMIT(Small_Yaw_Ctrl_Cmd.pitch, SMALL_YAW_2_PITCH_MIN_ANGLE, SMALL_YAW_2_PITCH_MAX_ANGLE);
                }
                break;

            default:
                break;
        }

        // 处理CH9和CH10的逻辑
        if (sbus.SBUS_CH.CH9 > SBUS_CHX_BIAS)
        {
            Small_Yaw_Ctrl_Cmd.gimbal_mode = GIMBAL_AUTO_MODE;
        }
        if (sbus.SBUS_CH.CH10 > SBUS_CHX_BIAS)
        {
            gimbal_cmd_send.gimbal_mode = GIMBAL_AUTO_MODE;
            Chassis_Ctrl_Cmd.chassis_mode  = CHASSIS_AUTO_MODE;
        }

        // 处理射击控制逻辑
        switch (sbus.SBUS_CH.CH5)
        {
            case SBUS_CHX_DOWN:
                Shoot_Ctrl_Cmd.shoot_mode = SHOOT_OFF;
                Shoot_Ctrl_Cmd.friction_mode = FRICTION_OFF;
                Shoot_Ctrl_Cmd.load_mode = LOAD_STOP;
                break;

            case SBUS_CHX_BIAS:
                Shoot_Ctrl_Cmd.shoot_mode = SHOOT_ON;
                Shoot_Ctrl_Cmd.friction_mode = FRICTION_OFF;
                break;

            case SBUS_CHX_UP:
                Shoot_Ctrl_Cmd.shoot_mode = SHOOT_ON;
                Shoot_Ctrl_Cmd.friction_mode = FRICTION_ON;
                if (sbus.SBUS_CH.CH7 == SBUS_CHX_DOWN)
                {
                    Shoot_Ctrl_Cmd.load_mode = LOAD_1_BULLET;
                }
                else if (sbus.SBUS_CH.CH7 == SBUS_CHX_BIAS)
                {
                    Shoot_Ctrl_Cmd.load_mode = LOAD_STOP;
                }
                else if (sbus.SBUS_CH.CH7 == SBUS_CHX_UP)
                {
                    Shoot_Ctrl_Cmd.load_mode = LOAD_BURSTFIRE;
                }
                else
                {
                    Shoot_Ctrl_Cmd.load_mode = LOAD_STOP;
                }
                Shoot_Ctrl_Cmd.shoot_rate = 20;
                break;

            default:
                break;
        }

        // 处理底盘控制逻辑
        switch (sbus.SBUS_CH.CH8)
        {
            case SBUS_CHX_DOWN:
                if(sbus.SBUS_CH.CH10 > SBUS_CHX_BIAS){return;}
                else {Chassis_Ctrl_Cmd.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;}
                break;

            case SBUS_CHX_BIAS:
                Chassis_Ctrl_Cmd.chassis_mode = CHASSIS_ROTATE;
                break;

            case SBUS_CHX_UP:
                Chassis_Ctrl_Cmd.chassis_mode = CHASSIS_ROTATE_REVERSE;
                break;

            default:
                break;
        }
    }
    else
    {
        // 处理离线状态
        gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
        Small_Yaw_Ctrl_Cmd.gimbal_mode = GIMBAL_ZERO_FORCE;
        Chassis_Ctrl_Cmd.chassis_mode = CHASSIS_ZERO_FORCE;
        Shoot_Ctrl_Cmd.shoot_mode = SHOOT_OFF;
        Shoot_Ctrl_Cmd.friction_mode = FRICTION_OFF;
        Shoot_Ctrl_Cmd.load_mode = LOAD_STOP;
    }
}

#endif


